#include <SimulationCore/SimMath.h>
#include <SimulationCore/Defines.h>


namespace SimulationCore
{
    namespace SimMath
    {

        void destination(double lat1Rad, double lon1Rad,
                            double bearingRad, double distance,
                            double &out_latRad, double &out_lonRad,
                            double radius)
        {
            double dR = distance / radius;
            out_latRad = asin( sin(lat1Rad)*cos(dR) + 
                            cos(lat1Rad)*sin(dR)*cos(bearingRad) );
            out_lonRad = lon1Rad + atan2(sin(bearingRad)*sin(dR)*cos(lat1Rad), 
                                        cos(dR)-sin(lat1Rad)*sin(out_latRad));
        }


         std::vector<SimPoint> GenerateCircle2D(double x, double y, double radius, int numSegments)
         {
            std::vector<SimPoint> circlePoints;
            if (numSegments < 11) {  
                return circlePoints;
            }
            if (radius <= 0) {
                return circlePoints;
            }

            const double twoPi = 2 * M_PI; 
            const double angleStep = twoPi / numSegments;  

            for (int i = 0; i < numSegments; ++i) {
                double currentAngle = (i * angleStep);
                double x1 = x + radius * sin(currentAngle);
                double y1 = y + radius * cos(currentAngle);
                circlePoints.emplace_back(SimPoint(x1, y1));
            }

            circlePoints.emplace_back(circlePoints[0]);

            return circlePoints;
         }

         std::vector<SimPoint> GenerateCircle3D(double x, double y,double z, double radius, int numSegments)
         {
            std::vector<SimPoint> circlePoints;
            if (numSegments < 11) {  
                return circlePoints;
            }
            if (radius <= 0) {    
                return circlePoints;
            }

            const double twoPi = 2 * M_PI; 
            const double angleStep = twoPi / numSegments;  

            double earthRadius = EARTH_RADIUS;
            double lat = x * DEG_TO_RAD;
            double lon = y * DEG_TO_RAD;
            double rM  = radius;

            for (int i = 0; i < numSegments; ++i) {
                double currentAngle = (i * angleStep);
                double clat, clon;
                destination( lat, lon, currentAngle, rM, clat, clon, earthRadius );
                circlePoints.emplace_back(SimPoint(clon * RAD_TO_DEG, clat * RAD_TO_DEG, z));
            }

            circlePoints.emplace_back(circlePoints[0]);

            return circlePoints;
         }
    }
}